#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <getopt.h>
#include <errno.h>

#include "coordMath.h"


/* This is a test driver for the coordMath library. Written for the SPSU
 * Aerial Robotics Team.
 *
 * basic operation type coordMath then provide two of the unit type options.
 *  -ecef, -llh, -ned (you must set -ned-ref if you use -ned)
 *
 *  the program will take the option that was provided with a point in the form '1 2 3'
 *  and convert it to another type.
 */
extern char* optarg;

static int verbose_flag;

static struct option long_options[] = 
{{"verbose", no_argument,       &verbose_flag, 1},
 {"help",    no_argument,       0,            'h'},
 {"ecef",    optional_argument, 0,            'e'},
 {"llh",     optional_argument, 0,            'l'},
 {"ned",     optional_argument, 0,            'n'},
 {"ned-ref", required_argument, 0,            'r'},
 {0,         0, 0,  0}};

int main(int argc, char* argv[]) {

    float one = 0.0;
    float two = 0.0;
    float three = 0.0;

    char convertFrom = 0;
    char convertTo = 0;

    int parse_failed = 0;

    float ned_ref1 = 0.0;
    float ned_ref2 = 0.0;
    int got_ned_ref = 0;

    while(1) {

        int option_index = 0;
        int c = getopt_long_only(argc, argv, "vhe::l::n::r:", long_options, &option_index);

        if(c == -1)
            break;

        switch(c) {
            case 0:
                break;

            case 'h':
                printf("useage: %s\n -type1='coordinate' -type2", argv[0]);
                puts("\tconverts coordinates given in type1 to type2. Latitude and Longitude");
                puts("\tshould be given in radians. All other mesurments should be given in");
                puts("\tmeters.\n\n\toptions:\n\t\t-verbose, -v  give verbose output");
                puts("\t\t-help, -h  print this help text\n\t\t-ecef, -e specify an ecef coordinate");
                puts("\t\t-llh, -l specify a llh coordinate\n");
                puts("\t\t-ned, -n specify a ned coordinate");
                puts("\t\t-ned-ref, -r (required when -ned is used) specify the latitude and longitude of the ref point");
                puts("\nif you provide more then one coordinate and one conversion type the results are undefined. (the");
                puts("last unit entered in eather case will probably win out"); 
                return 0;

            case 'e':
            case 'l':
            case 'n':
                if(optarg != NULL) { /* check if optarg is 0 */
                    if(sscanf(optarg, "%f%f%f", &one, &two, &three) != 3) {
                        puts("error: could not parse the coordinates");
                        parse_failed = 1;
                    } else {
                    convertFrom = c;
                    } /* end if-else */
                } else {
                    convertTo = c;
                } /* if-end else */
                break;
            
            case 'r':
                got_ned_ref = 1;
                if(sscanf(optarg, "%f%f", &ned_ref1, &ned_ref2) != 2) {

                    puts("error: could not parse the ned-ref paramiter");
                    parse_failed = 1;
                    got_ned_ref = 0;

                } /* end if */
                break;

            default:
                return 1;    
        } /* end switch */
    } /* end while */

    /* flag indicates weather an error has been encountered or not */
    int error = 0;

    if(convertFrom == 0 ) {
        puts("error: you must provide a coordinate to be converted");
        error = 1;
    } /* end if */

    if(convertTo == 0) {
        puts("error: you must indicate what you want the coordinate converted to");
        error = 1;
    } /* end if */
        
    if((convertTo == 'n' || convertFrom == 'n') && !got_ned_ref) {
        puts("error: to convert to or from ned you must provide the reference point in llh");
        error = 1;
    } /* end if */

    if(error == 1 || parse_failed)
        return 1;

    LLHvector  llh = {one, two, three};
    LLHvector  ref = {ned_ref1, ned_ref2, 0.0};
    ECEFvector ecef = {one, two, three};
    NEDvector  ned =  {one, two, three};

    if(convertTo == 'l' && convertFrom == 'e') {
        llh = ECEFtoLLH(ecef);
        printf("(%f, %f, %f)\n", llh.lat, llh.lon, llh.height);
    } /* end if */

    else if(convertTo == 'e' && convertFrom == 'l') {
        ecef = LLHtoECEF(llh);
        printf("(%f, %f, %f)\n", ecef.x, ecef.y, ecef.z);
    } /* end if */

    else if(convertTo == 'e'&& convertFrom == 'n') {
        ecef = NEDtoECEF(ned, ref);
        printf("(%f, %f, %f)\n", ecef.x, ecef.y, ecef.z);
    } /* end if */

    else if(convertTo == 'n' && convertFrom == 'e') {
        ned = ECEFtoNED(ecef, ref);
        printf("(%f, %f, %f)\n", ned.north, ned.east, ned.down);
    } /* end if */

    else if(convertTo == 'l' && convertFrom == 'n') {
        llh = ECEFtoLLH(NEDtoECEF(ned, ref));
        printf("(%f, %f, %f)\n", llh.lat, llh.lon, llh.height); 
    } /* end if */

    else if(convertTo == 'n' && convertFrom == 'l') {
        ned = ECEFtoNED(LLHtoECEF(llh), ref);
        printf("(%f, %f, %f)\n", ned.north, ned.east, ned.down);
    } /* end else if */

    return 0;
} /* end main */
